工具动力学/重力参数自动辨识接口说明

目前基于电流的负载参数辨识分为三种类型：type = TOOL_JNT_CURRENT(基于关节电流的工具动力学参数辨识)，type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS(基于关节电流的工具重力参数辨识-单向轨迹)，type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP(基于关节电流的工具重力参数辨识-往返轨迹)，其基本流程和接口如下：

1.设置相关参数

1.1 建立机器人算法接口实例

/**
* @brief 初始化算法库, 生成机器人模型
* @param urdfModel: urdf模型, 以字符串形式描述
* @param srdfModel: srdf模型, 以字符串形式描述
* @return < 0, 表示初始化失败
*/
ARAL_API_BASIC(1.0) int rlInitiateRobotModel(const std::string& urdfModel, const std::string& srdfModel) = 0;

/**
* @brief 初始化算法库, 设置机械臂模型及配置参数信息(算法会自动加载对应的文件)
* @param robot_name: 机械臂名称
* @param path: urdf, meshes等资源文件夹所在的路径(如./aral_export/aubo_description/)
*/
ARAL_API_BASIC(1.0) int rlInitiateRobotModelFromFiles(const std::string& robot_name, const std::string& path) = 0;

1.2 设置力矩常数

/**
* @brief 设置机械臂各关节的力矩常数, 单位: (Nm/A)
* @param moduleConstants: 力矩常数, 向量的维度应该和关节个数一致
*/
ARAL_API_COMMON(1.0) void mdlSetJointTorqueConstant(const RLJntArray& moduleConstants) = 0;

1.3设置摩擦力参数

 /**
 * @brief 设置关节摩擦参数, 该参数一般通过读取关节驱动中的参数而获得
 * @param friPara: 摩擦模型参数结构体, 长度为机器人自由度(一般是6), 顺序为[J1,J2,J3,J4,J5,J6]
 *                 类型为 VTL_Model 速度-温度-载荷模型
 *                 具体参数及顺序为[Fs, Fc, Vs, Miu, Fv0, Fv1, Fv2, Fv3, Ft1, Ft2, Ft3, c1, c2]
 * @return < 0, 表示设置失败
 */
ARAL_API_COMMON(1.0) int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara) = 0;

1.4 设置重力加速度

/**
 * @brief 设置重力加速度向量在机械臂基坐标系下的描述
 * @param vec[0]: 重力加速度在机械臂基坐标系X轴上的分量
 *        vec[1]: 重力加速度在机械臂基坐标系Y轴上的分量
 *        vec[2]: 重力加速度在机械臂基坐标系Z轴上的分量
 * @return if < 0, 表示设置失败
 */
ARAL_API_COMMON(1.0) int mdlSetGravityVectorInBase(const Array3d& vec) = 0;

1.5设置动力学参数

/**
  * @brief 设置机械臂的连杆动力学参数
  * @param real_para: 实际辨识出来的参数，参数格式为: real_para = [参数类型, 辨识的参数]
  *                   1. real_para[0] = 0; size(real_para) = 60 + 1, 输入参数含义为 m, mx, my, mz, ixx, ixy, ixz, iyy, iyz, izz ..., 需要另外设置转子惯量．否则为urdf里面的值．
  *                   2. real_para[0] = 1; size(real_para) = 58 + 1, 对应　aral　动力学标定接口的数据输出格式.
  *                   3. real_para[0] = 2; size(real_para) = 42 + 1, 对应第一版拖动示教导入生产写在底座的数据格式．
  * @return: if < 0, 则设置的参数类型或大小不正确
  */
ARAL_API_COMMON(1.0) int mdlSetRobotLinkDynamicParameter(const std::vector<double>& real_para) = 0;

1.6 设置激励轨迹和辨识配置信息

struct CalibConfig
{
    DynCalibType        type;                                               // 动力学参数辨识类型
    std::vector<int>    move_axis;                                          // 运动的轴(ID), 下标从0开始, 如[3, 5]代表运动关节为4,6关节
    std::vector<double> init_joint;                                         // 轨迹的起始关节角
    int                 sample_frequency{ARAL_TRAJECTORY_SAMPLE_FREQUENCY}; // 测量轨迹的采样频率
};//工具标定配置信息

其中type = TOOL_JNT_CURRENT(基于关节电流的工具动力学参数辨识) 需要设置配置信息，用于生成激励轨迹及用于负载动力学参数辨识type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS(基于关节电流的工具重力参数辨识-单向轨迹)和type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP(基于关节电流的工具重力参数辨识-往返轨迹)不需要设置配置信息用于负载重力参数辨识。

2.生成激励轨迹

2.1 生成激励轨迹

• 基于关节电流的工具动力学参数辨识

type = TOOL_JNT_CURRENT(基于关节电流的工具动力学参数辨识)需要设置激励轨迹系数，type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS(基于关节电流的工具重力参数辨识-单向轨迹)和type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP(基于关节电流的工具重力参数辨识-往返轨迹)不需要设置激励轨迹系数，生成激励轨迹接口如下：

/**
 * @brief 根据配置信息生成激励轨迹
 * @param config: 具体的配置信息, 根据实际辨识需求设置(参考 CalibConfig 结构体)
 * @param excitation_trajectory: 辨识的激励轨迹(需要运行该轨迹得到测量数据)
 * @param trajectory_para: 输出的激励轨迹的系数(在辨识动力学参数时算法需要输入该参数进行计算)
 * @return: 返回值 < 0, 表示计算失败(需要检查配置信息中的约束条件是否过于严格或者相互矛盾)
 */
ARAL_API_COMMON(1.0) int calibGenerateExcitationTrajectory(const CalibConfig& config, DoubleVecVec& excitation_trajectory, DoubleVec& trajectory_para) = 0;

由于实际用户现场可能不希望在负载辨识时控制机械臂多关节大幅度运动，故最好选用config.move_axis = {3, 5}

config.init_joint: 运动初始关节角。为了避免关节5接近零位时的奇异问题，应设置config.init_joint[4]的绝对值不小于0.3(rad)，接近1.57(rad)为宜。其余关节的关节角可任意设置。

config.sample_frequency:采样频率，和接口板的硬件采样频率保持一致，当前版本为200(Hz)

• 基于关节电流的工具重力参数辨识(单向轨迹/往返轨迹)

通过设置三个不同的位姿点，机器人按照位姿进行运动来估算其末端安装的有效负载和质心，设置位姿点时要求三个点位姿差异较大，并保证至少有三个关节角度存在显著变化。

推荐位姿如下：

其中，红色为x轴，绿色为y轴，蓝色为z轴，参考坐标系为世界坐标系。

在采用 moveJ 进行轨迹规划后，将规划后的数据与其逆序数据组合为一组新的辨识轨迹，以便于后续数据的更好对齐。

注意事项:

• 运动速度设为0.1rad/s，type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS(基于关节电流的工具重力参数辨识-单向轨迹)单向运动1次，type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP(基于关节电流的工具重力参数辨识-往返轨迹)轨迹往返运动1次;
• 进行负载重力辨识前建议降低碰撞等级以避免运行过程中的误碰撞检测;
• 运动过程中保证机械臂不发生自碰撞，建议采用拖动示教方式设置位姿点。

2.2 轨迹写入文件

软件部完成此步骤 基于关节电流的工具动力学参数辨识：记录excitation_trajectory: 辨识的激励轨迹和trajectory_para: 输出的激励轨迹的系数

基于关节电流的工具重力参数辨识(单向轨迹/往返轨迹)：记录机械臂运动过程中的相关数据。

3. 轨迹预筛选，判断轨迹是否可用(仅基于关节电流的工具重力参数辨识(单向轨迹/往返轨迹)有此功能)

3.1 读取规划数据

调用以下接口读取规划轨迹

/**
 * @brief 规划器在某条路径上以分辨率长度插值得到的一系列路径点
 * @param path_info: 描述多条路径的几何信息，路径属性，运动属性以及工具和工件信息，其每个元素 MotionProperty 用于创建一条基本路径（直线、圆弧、样条...）,
 *                   通过在PathProperty设置交融半径大小，路径之间可进行交融形成组合路径(目前支持直线、圆弧和关节之间的交融）
 * @param s: 分辨率长度, 单位m
 * @param interPoints: 插值的路径点
 * @return if < 0, 则表示计算出错
 */
ARAL_API_COMMON(1.0) int tpGetPathPointAtGivenResolutionLength(const std::vector<MotionProperty>& path_info, const double& s, std::vector<PathPoint>& interPoints) const = 0;

读取激励轨迹数据中的关节角q，其余角速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base数据均置0。

3.2 轨迹预筛选

调用以下接口

ARAL_API_COMMON(1.0) int calibToolDynamicParameter(const CalibTrajectoryFeature& traj_feature, const DoubleVecVec& data_raw, ToolCalibResult& result) = 0;

该接口根据传入data_raw中19-24列数据(电流)的值来选择该接口是进行轨迹预筛选功能还是负载辨识功能。

实现轨迹预筛选功能时，输入输出如下：

/**
 * @brief 工具动力学参数辨识数据预筛选
 *        说明: 根据规划得到的辨识轨迹，判断辨识轨迹是否可用
 * @param traj_feature: 轨迹特征信息, 具体包括如下
 *        traj_feature.config: 激励轨迹配置信息(辨识类型，初始关节角，运动轴以及采样频率具体参考 CalibConfig 结构体)
 *                            可辨识类型type包括:
 *                            type = TOOL_GRAVITY_JNT_CURRENT:  基于关节电流的工具重力参数辨识
 * @param data_raw: 关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base
 *             type = TOOL_GRAVITY_JNT_CURRENT时传入数据为
 *                                      q1 ... q6, 0...0,...，0 ... 0 带载数据，共dof*7列，n行
 * @param result:   1. inertia: 工具在法兰坐标系下的惯性参数(具体参考 RLInertia 结构体)
                                struct RLInertia
                                {
                                    double      mass;         // 刚体质量
                                    Array3d     center;       // 刚体质心位置（3*1数组）
                                    Array6d     inertia;      // 刚体惯性参数, 绕质心转动，对齐到参考坐标系, 顺序为 [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]）
                                 };// 刚体动力学参数结构体
 *                  2. error: 辨识误差, 辨识参数映射到关节力矩/末端六维力 与 实际力矩/六维力的误差, 包括误差的average, max, rms值
 * @return 返回值 > 0, 表示该辨识轨迹可用，可以进行辨识
 *            返回值 < 0, 表示该轨迹不可用，建议增加运动轴
 */
ARAL_API_COMMON(1.0) int calibToolDynamicParameter(const CalibTrajectoryFeature& traj_feature, const DoubleVecVec& data_raw, ToolCalibResult& result) = 0;

4.运行激励轨迹，采集数据

4.1 采集数据

基于关节电流的工具动力学参数辨识：空载运行激励轨迹，带载运行激励轨迹，记录以下数据：关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base

 *             type = TOOL_JNT_CURRENT时记录数据格式如下：
 *             q1 ... q6, qd1...qd6,...，F_base1 ... F_base6不带载第一组数
 *             .
 *             .
 *             .
 *             q1 ... q6, qd1...qd6,...，F_base1 ... F_base6不带载第n组数
 *             q1 ... q6, qd1...qd6,...，F_base1 ... F_base6带载第一组数
 *             .
 *             .
 *             .
 *             q1 ... q6, qd1...qd6,...，F_base1 ... F_base6带载第n组数

基于关节电流的工具重力参数辨识(单向轨迹/往返轨迹)：带载运行激励轨迹，记录以下数据：关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base

 *             type = TOOL_GRAVITY_JNT_CURRENT时记录数据格式如下：
 *             q1 ... q6, qd1...qd6,...，F_base1 ... F_base6 带载数据，共n行

软件部完成此步骤

4.2 加载采集的数据

软件部完成此步骤

5.参数辨识

5.1 设置摩擦力参数

ARAL_API_COMMON(1.0) int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara) = 0;

ps: 3.2轨迹预筛选时函数内部更改了摩擦力参数，需要重新设置一下原始的摩擦力参数

5.2 调用如下接口，实现辨识负载功能

/**
 * @brief 工具动力学参数辨识
 *        说明: 辨识步骤为先设置 CalibConfig 信息, 调用 calibGenerateExcitationTrajectory 得到激励轨迹及激励轨迹系数, 再运行激励轨迹得到测量数据,
 *             这样就可以得到该函数的输入 traj_feature 及 data_raw, 最后调用该函数进行参数辨识.
 * @param traj_feature: 轨迹特征信息, 具体包括如下
 *        traj_feature.config: 激励轨迹配置信息(辨识类型，初始关节角，运动轴以及采样频率具体参考 CalibConfig 结构体), 该信息需与 calibGenerateExcitationTrajectory 函数调用时的信息一致
 *                            可辨识类型type包括:
 *                            type = TOOL_JNT_CURRENT:          基于关节电流的工具动力学参数辨识
 *                            type = TOOL_END_SENSOR:           基于末端力传感器的工具动力学参数辨识
 *                            type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS: 基于关节电流的工具重力参数辨识(单向轨迹)
 *                            type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP: 基于关节电流的工具重力参数辨识(往返轨迹)
 *        traj_feature.coeff: 激励轨迹系数, 由 calibGenerateExcitationTrajectory 函数计算得来. 不采用激励轨迹的类型不用设置
 * @param data_raw: 关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base
 *             type = TOOL_JNT_CURRENT 时传入数据如下
 *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6不带载第一组数
 *                                                  .
 *                                                  .
 *                                                  .
 *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6不带载第n组数
 *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6带载第一组数
 *                                                  .
 *                                                  .
 *                                                  .
 *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6带载第n组数
 *             type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS 或
 *             type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP 时传入数据为
 *                                      q1 ... q6, qd1...qd6,...，F_base1 ... F_base6 带载数据，共n行
 * @param result:   1. inertia: 工具在法兰坐标系下的惯性参数(具体参考 RLInertia 结构体)
                                struct RLInertia
                                {
                                    double      mass;         // 刚体质量
                                    Array3d     center;       // 刚体质心位置（3*1数组）
                                    Array6d     inertia;      // 刚体惯性参数, 绕质心转动，对齐到参考坐标系, 顺序为 [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]）
                                 };// 刚体动力学参数结构体
 *                  2. error: 辨识误差, 辨识参数映射到关节力矩/末端六维力 与 实际力矩/六维力的误差, 包括误差的average, max, rms值
 * @return 返回值 < 0, 表示辨识计算失败
 *         返回值 > 0, 表示辨识计算成功 
 */
ARAL_API_COMMON(1.0) int calibToolDynamicParameter(const CalibTrajectoryFeature& traj_feature, const DoubleVecVec& data_raw, ToolCalibResult& result) = 0;

6.调用示例

TEST_FIXTURE(AuboRobotInterface, testToolDynamicIdentify)
{
    //! step1: 设置相关参数
    //! step1.1: 设置机器人类型
    Setup("aubo_i5");
    //! step1.1: 设置力矩系数
    robot->mdlSetJointTorqueConstant(motor_const);
    //! step1.3: 设置摩擦参数
    robot->mdlSetJointFrictionParameter(friction_para);
    //! step1.4: 设置重力加速度
    robot->mdlSetGravityVectorInBase(gravity_acc);
    //! step1.5: 设置动力学参数
    robot->mdlSetRobotLinkDynamicParameter(link_para);
    //! step1.6: 设置激励轨迹和辨识配置参数
    interface::CalibConfig excite_traj_config;
    excite_traj_config.type         = 80;
    excite_traj_config.move_axis    = {3, 4, 5};
    excite_traj_config.init_joint   = {0, 0, 0, 0, 0, 0};
    excite_traj_config.sample_frequency = 200;
    std::vector<double> traj_parameter;
    trajFeature.coeff = traj_parameter;
    trajFeature.config = excite_traj_config;

    //! step2: 辨识轨迹预判断
    interface::ToolCalibResult paraEst;
    //data_raw数据格式如下: q1 ... q6, 0...0,...，0 ... 0 带载数据，共n行,dof*7列
    ret = robot->calibToolDynamicParameter(trajFeature, data_raw, paraEst);

    //! step3.1: 设置摩擦参数
    robot->mdlSetJointFrictionParameter(friction_para);
    //! step3.2: 负载辨识
    robot->mdlSetJointFrictionParameter(friction_para);
    interface::ToolCalibResult paraEst;
    //data_raw数据格式如下: q1 ... q6, qd1...qd6,...，F_base1 ... F_base6 带载数据，共n行，dof*7列
    ret = robot->calibToolDynamicParameter(trajFeature, data_raw, paraEst);

    //! step4: 校验 (cad)理论参数与辨识参数校验
    double iden_error_range = 0.6;
    CHECK_ARRAY_CLOSE(cad_values, paraEst, cad_values.size(), iden_error_range);
}

7.其他说明

• 开始辨识前确认相关参数正确设置
• 进行基于关节电流的工具动力学参数辨识时，建议参数辨识在热机一段时间后再进行，空载和带载辨识的时间间隔尽量短，避免温度对辨识精度造成影响